#include <settings.hpp>
#include <utils.hpp>
//
#include <remoter.hpp>
#include <remoter_cali_normal.hpp>

RemoterCaliNormal::RemoterCaliNormal(int state)
	: RemoterCali(state)
	, m_NormalFilter(0.3)
{
	memset(m_RcPrev, 0, sizeof(double) * MSG_RC_E_CNT);
}

RemoterCaliNormal::~RemoterCaliNormal()
{
}

void RemoterCaliNormal::Process(msg_rc_raw_s* rcRaw, msg_rc_s* rc)
{
	RemoterCali::Process(rcRaw, rc);

	int8_t indThro	= g_Settings->rc_ind_thro;
	int8_t indRoll	= g_Settings->rc_ind_roll;
	int8_t indPitch = g_Settings->rc_ind_pitch;
	int8_t indYaw	= g_Settings->rc_ind_yaw;

	double ctl[MSG_RC_E_CNT] = { 0, 0, 0, 0 };

	ctl[MSG_RC_E_THRO] = CalcValueByParams(rcRaw->rc_raw[indThro], //
										   g_Settings->rc_cali_thro_min,
										   g_Settings->rc_cali_thro_mid,
										   g_Settings->rc_cali_thro_max,
										   g_Settings->rc_cali_thro_reverse);

	ctl[MSG_RC_E_ROLL] = CalcValueByParams(rcRaw->rc_raw[indRoll], //
										   g_Settings->rc_cali_roll_min,
										   g_Settings->rc_cali_roll_mid,
										   g_Settings->rc_cali_roll_max,
										   g_Settings->rc_cali_roll_reverse);

	ctl[MSG_RC_E_PITCH] = CalcValueByParams(rcRaw->rc_raw[indPitch], //
											g_Settings->rc_cali_pitch_min,
											g_Settings->rc_cali_pitch_mid,
											g_Settings->rc_cali_pitch_max,
											g_Settings->rc_cali_pitch_reverse);

	ctl[MSG_RC_E_YAW] = CalcValueByParams(rcRaw->rc_raw[indYaw], //
										  g_Settings->rc_cali_yaw_min,
										  g_Settings->rc_cali_yaw_mid,
										  g_Settings->rc_cali_yaw_max,
										  g_Settings->rc_cali_yaw_reverse);

	rc->ctl[MSG_RC_E_THRO]	= FilterLowPass(ctl[MSG_RC_E_THRO], m_RcPrev[MSG_RC_E_THRO], m_NormalFilter);
	rc->ctl[MSG_RC_E_ROLL]	= FilterLowPass(ctl[MSG_RC_E_ROLL], m_RcPrev[MSG_RC_E_ROLL], m_NormalFilter);
	rc->ctl[MSG_RC_E_PITCH] = FilterLowPass(ctl[MSG_RC_E_PITCH], m_RcPrev[MSG_RC_E_PITCH], m_NormalFilter);
	rc->ctl[MSG_RC_E_YAW]	= FilterLowPass(ctl[MSG_RC_E_YAW], m_RcPrev[MSG_RC_E_YAW], m_NormalFilter);

#if 0
	if (m_Tk % 10 == 0)
	{
		LOGL("%+5.3lf %+6.3lf %+5.3lf %+5.3lf %08x\n", //
			 rc->ctl[MSG_RC_E_THRO],
			 rc->ctl[MSG_RC_E_ROLL],
			 rc->ctl[MSG_RC_E_PITCH],
			 rc->ctl[MSG_RC_E_YAW],
			 rc->sw_btn);
	}
#endif
}